//
// Created by wangguan on 23-3-3.
// use it to define the start state by use the global varible!
//
#include "initial.h"
// 文件设置
//std::string gnss_file = "/home/wangguan/Desktop/data2/Gnss/gps_pos_0.txt";
//std::string imu_file = "/home/wangguan/Desktop/data2/Imu/imu_er_0.bin";
//std::string save_path = "/home/wangguan/Desktop/data2"; // 不能是文件名字！
//
//// imu误差设置
//double ARW = 0.24 / 180.0 * M_PI / 60.0;      //deg/sqrt(hr) to rad/sqrt(s)
//double VRW = 0.24 / 60.0;                     //m/s/sqrt(hr) to m/s/sqrt(s) ??????； m/s/sqrt() = m/sqrt(s^3)
//double gbStd = 50.0 / 180.0 * M_PI / 3600.0;    //deg/hr to rad/s
//double abStd = 250.0 * 1e-5;                   // mGal to M/S^2
//double gsStd = 1000.0 * 1e-6;                   // ppm to
//double asStd = 1000.0 * 1e-6;                   // ppm to 百万分之一
//double gnStd = 500.0 * 1e-6;                 // 陀螺仪交轴耦合???????
//double anStd = 500.0 * 1e-6;                 // 加速度计交轴耦合??????
//// 4hr to 4 * 3600s
//double Tgb_reci = 1.0 / 3600.0;
////4hr to 4 * 3600s
//double Tab_reci = 1.0 / 3600.0;
////4hr to 4 * 3600s
//double Tgs_reci = 1.0 / 3600.0;
////4hr to 4 * 3600s
//double Tas_reci = 1.0 / 3600.0;
////Tgn??????
//double Tgn_reci = 1.0 / 3600.0;
////Tan???????
//double Tan_reci = 1.0 / 3600.0;
////杆臂
//Eigen::Vector3d l2b(0.1, 0.1, 0.1);
////T_imu
//double T_imu = 1.0 / 200.0;
//
//// 初始位置设置
//// 初始pva
//double init_roll =      1.02023805     / 180.0 * M_PI;
//double init_pitch =     -2.00503030    / 180.0 * M_PI;
//double init_heading =   185.70224431   / 180.0 * M_PI;
//double init_v_n =       0.0000       ;
//double init_v_e =       0.0000       ;
//double init_v_d =       0.0000       ;
//double init_latitude =  30.444787374    / 180.0 * M_PI;
//double init_longitude = 114.471863202   / 180.0 * M_PI;
//double init_height =    20.889000    ;
//double init_time =      455861.010;
////因为速度姿态位置的标准差是会动态变化的
//double pos_n_std_init = 0.5;
//double pos_e_std_init = 0.5;
//double pos_d_std_init = 0.5;
//double vel_n_std_init = 0.1;
//double vel_e_std_init = 0.1;
//double vel_d_std_init = 0.1;
//double att_n_std_init = 1.0 / 180.0 * M_PI;
//double att_e_std_init = 1.0 / 180.0 * M_PI;
//double att_d_std_init = 5.0 / 180.0 * M_PI;
//
//// 矩阵的维数
//int n; // 这个不需要设置
//// 矩阵哪些维度需要关掉，默认全部关闭，如果需要打开的话在main函数里面打开
//int switch_off[24] = {-1, -1, -1, -1,
//                      -1, -1, -1, -1,
//                      -1, -1, -1, -1,
//                      -1, -1, -1, -1,
//                      -1, -1, -1, -1,
//                      -1, -1, -1, -1};
//
//// 中断开始时间
//int inter_start_time[3] = {1001, 1061, 1121};
//// 中断周期
//int inter_circle = 180;
//// 中断持续时间
//int inter_length = 60;
//// 中断是否打开
//bool gnss_inter = true; // 默认打开
//
//// choose the 投影 mode
//GyroNonMode gyro_non_mode = gyro_down_triangle;
//AccelNonMode accel_non_mode = accel_down_triangle;
//ProjectMode project_mode = project2accel;
//
//// 初始 imu 误差
//// 需要另外设置一些值时候在函数里面设置
//Eigen::Vector<double, 24> init_imu_error = Eigen::Vector<double, 24>::Zero(); // 需要另外设置一些值时候在函数里面
//
//// 初始化 P 阵
//double std_Dev[33]; // 在函数里面初始化
//
//// 初始化 Q 阵；Q阵的初始化我就暂时不列入这里因为可以直接通过更改初始标准差来进行修改









// 文件设置
std::string gnss_file;
std::string imu_file;
std::string save_path; // 不能是文件名字！

// imu误差设置
double ARW ;      // deg/sqrt(hr) to rad/sqrt(s)
double VRW;       // m/sqrt(hr) to m/sqrt(s)
double gbStd;     // deg/hr to rad/s
double abStd;     // mGal to M/S^2
double gsStd;     // ppm to
double asStd;     // ppm to 百万分之一
double gnStd;     // 陀螺仪非正交角
double gmaStd;    // 陀螺仪安装角
double anStd;     // 加速度计非正交角
double amaStd;    // 加速度计安装角
// 4hr to 4 * 3600s
double Tgb_reci;
// 4hr to 4 * 3600s
double Tab_reci;
// 4hr to 4 * 3600s
double Tgs_reci;
// 4hr to 4 * 3600s
double Tas_reci;
// Tgn
double Tgn_reci;
// TgMA
double Tgma_reci;
// Tan
double Tan_reci;
// TaMA
double Tama_reci;
// 杆臂
Eigen::Vector3d l2b;
// T_imu
double T_imu;

// 初始位置设置
// 初始pva
double init_roll;
double init_pitch;
double init_heading;
double init_v_n;
double init_v_e;
double init_v_d;
double init_latitude;
double init_longitude;
double init_height;
double init_time;
//因为速度姿态位置的标准差是会动态变化的
double pos_n_std_init;
double pos_e_std_init;
double pos_d_std_init;
double vel_n_std_init;
double vel_e_std_init;
double vel_d_std_init;
double att_n_std_init;
double att_e_std_init;
double att_d_std_init;

// 矩阵的维数
int n; // 这个不需要设置
// 矩阵哪些维度需要关掉，默认全部关闭，如果需要打开的话在main函数里面打开
int switch_off[24];

// 中断开始时间
int inter_start_time[3];
// 中断周期
int inter_circle;
// 中断持续时间
int inter_length;
// 中断是否打开
bool gnss_inter; // 默认打开

// choose the 投影 mode
GyroNonMode gyro_non_mode;
AccelNonMode accel_non_mode;
ProjectMode project_mode;

// 初始 imu 误差
// 需要另外设置一些值时候在函数里面设置
Eigen::Vector<double, 24> init_imu_error = Eigen::Vector<double, 24>::Zero(); // 需要另外设置一些值时候在函数里面

// 初始化 P 阵
double std_Dev[33]; // 在函数里面初始化

// 初始化 Q 阵；Q阵的初始化我就暂时不列入这里因为可以直接通过更改初始标准差来进行修改



bool LoadConfig(std::string config_file) {
    bool flag = false;
    // read file
    YAML::Node config;
    try {
        config = YAML::LoadFile(config_file);
    } catch (YAML::Exception &exception) {
        std::cout << "Loading config file error!";
        return flag;
    }

    // 文件设置
    gnss_file = config["gnss_file"].as<std::string>();
    imu_file = config["imu_file"].as<std::string>();
    save_path = config["save_path"].as<std::string>();
    // check the save path
    if (reinterpret_cast<const char *>(save_path.back()) != "/") {
        save_path += "/";
    }

    // imu 误差设置
    ARW = config["ARW"].as<double>() / 180.0 * M_PI / 60.0;      //deg/sqrt(hr) to rad/sqrt(s)
    VRW = config["VRW"].as<double>() / 60.0;
    gbStd = config["gbStd"].as<double>() / 180.0 * M_PI / 3600.0;
    abStd = config["abStd"].as<double>() * 1e-5;
    gsStd = config["gsStd"].as<double>() * 1e-6;
    asStd = config["asStd"].as<double>() * 1e-6;
    gnStd = config["gnStd"].as<double>() * 1e-6;
    gmaStd = config["gmaStd"].as<double>() * 1e-6;
    anStd = config["anStd"].as<double>() * 1e-6;
    amaStd = config["amaStd"].as<double>() * 1e-6;
    Tgb_reci = config["Tgb_reci"].as<double>() / 3600.0;
    Tab_reci = config["Tab_reci"].as<double>() / 3600.0;
    Tgs_reci = config["Tgs_reci"].as<double>() / 3600.0;
    Tas_reci = config["Tas_reci"].as<double>() / 3600.0;
    Tgn_reci = config["Tgn_reci"].as<double>() / 3600.0;
    Tgma_reci = config["Tgma_reci"].as<double>() / 3600.0;
    Tan_reci = config["Tan_reci"].as<double>() / 3600.0;
    Tama_reci = config["Tama_reci"].as<double>() / 3600.0;

    // 杆臂
    l2b[0] = config["l2b"][0].as<double>();
    l2b[1] = config["l2b"][1].as<double>();
    l2b[2] = config["l2b"][2].as<double>();

    // T_IMU
    T_imu = 1.0 / config["F_imu"].as<double>();

    // 初始位置设置
    init_roll = config["init_roll"].as<double>() / 180.0 * M_PI;
    init_pitch = config["init_pitch"].as<double>() / 180.0 * M_PI;
    init_heading = config["init_heading"].as<double>() / 180.0 * M_PI;
    init_v_n =  config["init_v_n"].as<double>();
    init_v_e =  config["init_v_e"].as<double>();
    init_v_d =  config["init_v_d"].as<double>();
    init_latitude =  config["init_latitude"].as<double>() / 180.0 * M_PI;
    init_longitude = config["init_longitude"].as<double>() / 180.0 * M_PI;
    init_height =    config["init_height"].as<double>();
    init_time =      config["init_time"].as<double>();
    // 初始位置的标准差
    pos_n_std_init = config["pos_n_std_init"].as<double>();
    pos_e_std_init = config["pos_e_std_init"].as<double>();
    pos_d_std_init = config["pos_d_std_init"].as<double>();
    vel_n_std_init = config["vel_n_std_init"].as<double>();
    vel_e_std_init = config["vel_e_std_init"].as<double>();
    vel_d_std_init = config["vel_d_std_init"].as<double>();
    att_n_std_init = config["att_n_std_init"].as<double>() / 180.0 * M_PI;
    att_e_std_init = config["att_e_std_init"].as<double>() / 180.0 * M_PI;
    att_d_std_init = config["att_d_std_init"].as<double>() / 180.0 * M_PI;

    // 设置初始时刻哪些维度关掉
    // 选择哪些 imu_误差进行增广
    // 一种24维度
    // 0-2 gyro_bias
    // 3-5 accel_bias
    // 6-8 gyro_scale
    // 9-11 accel_scale
    // 12-14 gyro 非正交，可能是上三角，也可能是下三角
    // 15-17 gyro 安装角，投影到加表上面
    // 18-20 accel 非正交，可能是上三角，可能是下三角
    // 21-23 accel 安装角，投影到陀螺仪上面
    // 当 project_mode = project2accel
    // 21-23 永远都是0
    // 当 project_mode = project2gyro
    // 15-17 永远都是0
    // 当 project_mode = noproject
    // 15-17 21-23永远都是0
    // 不能只设置 noproject
    for (int i = 0; i < 24; i++) {
        switch_off[i] = config["switch_off"][i].as<int>();
    }

    // 中断开始时间
    inter_start_time[0] = config["inter_start_time"][0].as<int>();
    inter_start_time[1] = config["inter_start_time"][1].as<int>();
    inter_start_time[2] = config["inter_start_time"][2].as<int>();
    // 中断周期
    inter_circle = config["inter_circle"].as<int>();
    // 中断持续时间
    inter_length = config["inter_length"].as<int>();
    // 中断是否打开
    gnss_inter = config["gnss_inter"].as<bool>();

    // 投影模型
    if (config["gyro_non_mode"].as<std::string>() == "gyro_down_triangle") {
        gyro_non_mode = gyro_down_triangle;
    } else if (config["gyro_non_mode"].as<std::string>() == "gyro_up_triangle") {
        gyro_non_mode = gyro_up_triangle;
    } else {
        std::cout << "Please input the right gyro nonlinearlity mode!" << std::endl;
        return flag;
    }
    if (config["accel_non_mode"].as<std::string>() == "accel_down_triangle") {
        accel_non_mode = accel_down_triangle;
    } else if (config["accel_non_mode"].as<std::string>() == "accel_up_triangle") {
        accel_non_mode = accel_up_triangle;
    } else {
        std::cout << "Please input the right accel nonlinearlity mode!" << std::endl;
        return flag;
    }
    if (config["project_mode"].as<std::string>() == "project2accel") {
        project_mode = project2accel;
    } else if(config["project_mode"].as<std::string>() == "project2gyro") {
        project_mode = project2gyro;
    } else if (config["project_mode"].as<std::string>() == "noproject"){
        project_mode = noproject;
    } else {
        std::cout << "Please input the right project mode!" << std::endl;
        std::cout << "We choose the default mode: noproject" << std::endl;
        project_mode = noproject;

    }

    // imu 初始误差
    if (config["is_set_init_imu_error"].as<bool>() == false) {
        // 什么都不干，默认是0
    } else if (config["is_set_init_imu_error"].as<bool>() == true) {
        for (int i = 0; i < 3; ++i) {
            double temp = config["init_imu_error"][i].as<double>();
            if (temp != -1) {
                init_imu_error[i] = temp / 180.0 * M_PI / 3600.0; // gb
            }
        }
        for (int i = 3; i < 6; ++i) {
            double temp = config["init_imu_error"][i].as<double>();
            if (temp != -1) {
                init_imu_error[i] = temp * 1e-5; // ga
            }
        }
        for (int i = 6; i < 9; ++i) {
            double temp = config["init_imu_error"][i].as<double>();
            if (temp != -1) {
                init_imu_error[i] = temp * 1e-6; // gs
            }
        }
        for (int i = 9; i < 12; ++i) {
            double temp = config["init_imu_error"][i].as<double>();
            if (temp != -1) {
                init_imu_error[i] = temp * 1e-6; // as
            }
        }
        for (int i = 12; i < 18; ++i) {
            double temp = config["init_imu_error"][i].as<double>();
            if (temp != -1) {
                init_imu_error[i] = temp * 1e-6; // gn
            }
        }
        for (int i = 18; i < 24; ++i) {
            double temp = config["init_imu_error"][i].as<double>();
            if (temp != -1) {
                init_imu_error[i] = temp * 1e-6; // an
            }
        }
    } else {
        std::cout << "Please input the 'is_set_init_imu_error', it can only be true or false!" << std::endl;
    }

    // imu 初始 P 阵
    // 初始化 std_Dev，而 std_Dev 用来初始化 P 阵
    std_Dev[0] = pos_n_std_init * pos_n_std_init;// r
    std_Dev[1] = pos_e_std_init * pos_e_std_init;
    std_Dev[2] = pos_d_std_init * pos_d_std_init;
    std_Dev[3] = vel_n_std_init * vel_n_std_init;//v
    std_Dev[4] = vel_e_std_init * vel_e_std_init;
    std_Dev[5] = vel_d_std_init * vel_d_std_init;
    std_Dev[6] = att_n_std_init * att_n_std_init;//seta
    std_Dev[7] = att_e_std_init * att_e_std_init;
    std_Dev[8] = att_d_std_init * att_d_std_init;
    std_Dev[9] = gbStd * gbStd;//bg
    std_Dev[10] = gbStd * gbStd;
    std_Dev[11] = gbStd * gbStd;
    std_Dev[12] = abStd * abStd;//ba
    std_Dev[13] = abStd * abStd;
    std_Dev[14] = abStd * abStd;
    std_Dev[15] = gsStd * gsStd;//sg
    std_Dev[16] = gsStd * gsStd;
    std_Dev[17] = gsStd * gsStd;
    std_Dev[18] = asStd * asStd;//sa
    std_Dev[19] = asStd * asStd;
    std_Dev[20] = asStd * asStd;
    // 现在的交轴耦合误差就包含了两个方面，非正交误差和安装角误差；但是二者的处事不确定度和误差参数是一致的，所以这里可以不改
    // if gyro use up triangle matrix，21 is ny2x； 22 is nz2x; 23 is nz2y
    // if gyro use down triangle matrix, 21 is nx2y; 22 is nx2z; 23 is ny2z
    // 24 is fai_x, 25 is fai_y, 26 is fai_z
    std_Dev[21] = gnStd * gnStd;
    std_Dev[22] = gnStd * gnStd;
    std_Dev[23] = gnStd * gnStd;
    std_Dev[24] = gmaStd * gmaStd;
    std_Dev[25] = gmaStd * gmaStd;
    std_Dev[26] = gmaStd * gmaStd;
    // 现在的交轴耦合误差就包含了两个方面，非正交误差和安装角误差；但是二者的处事不确定度和误差参数是一致的，所以这里可以不改
    // if gyro use up triangle matrix，27 is ny2x； 28 is nz2x; 29 is nz2y
    // if gyro use down triangle matrix, 27 is nx2y; 28 is nx2z; 29 is ny2z
    // 30 is fai_x, 31 is fai_y, 32 is fai_z
    std_Dev[27] = anStd * anStd;
    std_Dev[28] = anStd * anStd;
    std_Dev[29] = anStd * anStd;
    std_Dev[30] = amaStd * amaStd;
    std_Dev[31] = amaStd * amaStd;
    std_Dev[32] = amaStd * amaStd;
    if (config["is_set_init_p"].as<bool>() == true) {
        for (int i = 0; i < 3; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp; // position 的单位就是标准单位，北向东向地向误差
            }
        }
        for (int i = 3; i < 6; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp; // vel 的单位就是标准单位
            }
        }
        for (int i = 6; i < 9; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        / 180.0 * M_PI / 180.0 * M_PI; // deg 2 rad
            }
        }
        for (int i = 9; i < 12; ++i) {
            double temp = config["init_p"][i].as<double>(); // bg
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        / 180.0 * M_PI / 3600.0 / 180.0 * M_PI / 3600.0;
            }
        }
        for (int i = 12; i < 15; ++i) {
            double temp = config["init_p"][i].as<double>(); // ba
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        * 1e-5 * 1e-5;
            }
        }
        for (int i = 15; i < 18; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        * 1e-6 * 1e-6; // sg
            }
        }
        for (int i = 18; i < 21; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        * 1e-6 * 1e-6; // sa
            }
        }
        for (int i = 21; i < 27; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        * 1e-6 * 1e-6; // gn
            }
        }
        for (int i = 27; i < 33; ++i) {
            double temp = config["init_p"][i].as<double>();
            if (temp != -1) {
                std_Dev[i] = temp * temp
                        * 1e-6 * 1e-6; // an
            }
        }
    } else {
        std::cout << "Please input the 'is_set_init_p', it can only be true or false!" << std::endl;
    }

    return true;
}

std::string GetSaveFilePrefix(std::string start_time) {
    int count = 0;
    std::string file_name_temp;
    if (project_mode == noproject) {
        file_name_temp += "noi_"; // 没有安装角
        count += 3;
    } else if (project_mode == project2gyro) {
        file_name_temp += "gyroi_";
    } else if (project_mode == project2accel) {
        file_name_temp += "acceli_";
    }
    if (gyro_non_mode == gyro_up_triangle) {
        file_name_temp += "gyroup_";
    } else if (gyro_non_mode == gyro_down_triangle) {
        file_name_temp += "gyrodown_";
    }
    if (accel_non_mode == accel_up_triangle) {
        file_name_temp += "accleup";
    } else if (accel_non_mode == accel_down_triangle) {
        file_name_temp += "acceldown";
    }
    for (int i = 0; i < 24; ++i) {
        if (switch_off[i] != -1) {
            switch (switch_off[i]) {
                case 0:
                    file_name_temp += "_gbx";
                    count++;
                    break;
                case 1:
                    file_name_temp += "_gby";
                    count++;
                    break;
                case 2:
                    file_name_temp += "_gbz";
                    count++;
                    break;
                case 3:
                    file_name_temp += "_abx";
                    count++;
                    break;
                case 4:
                    file_name_temp += "_aby";
                    count++;
                    break;
                case 5:
                    file_name_temp += "_abz";
                    count++;
                    break;
                case 6:
                    file_name_temp += "_gsx";
                    count++;
                    break;
                case 7:
                    file_name_temp += "_gsy";
                    count++;
                    break;
                case 8:
                    file_name_temp += "_gsz";
                    count++;
                    break;
                case 9:
                    file_name_temp += "_asx";
                    count++;
                    break;
                case 10:
                    file_name_temp += "_asy";
                    count++;
                    break;
                case 11:
                    file_name_temp += "_asz";
                    count++;
                    break;
                case 12:
                    if (gyro_non_mode == gyro_up_triangle) {
                        file_name_temp += "_gny2x";
                    } else if (gyro_non_mode == gyro_down_triangle) {
                        file_name_temp += "_gnx2y";
                    }
                    count++;
                    break;
                case 13:
                    if (gyro_non_mode == gyro_up_triangle) {
                        file_name_temp += "_gnz2x";
                    } else if (gyro_non_mode == gyro_down_triangle) {
                        file_name_temp += "_gnx2z";
                    }
                    count++;
                    break;
                case 14:
                    if (gyro_non_mode == gyro_up_triangle) {
                        file_name_temp += "_gnz2y";
                    } else if (gyro_non_mode == gyro_down_triangle) {
                        file_name_temp += "_gny2z";
                    }
                    count++;
                    break;
                case 15:
                    if (project_mode == project2accel) {
                        file_name_temp += "gix";
                        count++;
                    }
                    break;
                case 16:
                    if (project_mode == project2accel) {
                        file_name_temp += "giy";
                        count++;
                    }
                    break;
                case 17:
                    if (project_mode == project2accel) {
                        file_name_temp += "giz";
                        count++;
                    }
                    break;
                case 18:
                    if (accel_non_mode == accel_up_triangle) {
                        file_name_temp += "_any2x";
                    } else if (accel_non_mode == accel_down_triangle) {
                        file_name_temp += "_anx2y";
                    }
                    count++;
                    break;
                case 19:
                    if (accel_non_mode == accel_up_triangle) {
                        file_name_temp += "_anz2x";
                    } else if (accel_non_mode == accel_down_triangle) {
                        file_name_temp += "_anx2z";
                    }
                    count++;
                    break;
                case 20:
                    if (accel_non_mode == accel_up_triangle) {
                        file_name_temp += "_anz2y";
                    } else if (accel_non_mode == accel_down_triangle) {
                        file_name_temp += "_any2z";
                    }
                    count++;
                    break;
                case 21:
                    if (project_mode == project2gyro) {
                        file_name_temp += "_aix";
                        count++;
                    }
                    break;
                case 22:
                    if (project_mode == project2gyro) {
                        file_name_temp += "_aiy";
                        count++;
                    }
                    break;
                case 23:
                    if (project_mode == project2gyro) {
                        file_name_temp += "_aiz";
                        count++;
                    }
                    break;
                default:
                    std::cout <<
                              "You input wrong switch_off choice, which must form 0 to 23, the default value is -1."
                              << std::endl;
            }
        }
    }
    count = 33 - count;
    std::string file_out_prefix;
    if (gnss_inter) {
        file_out_prefix = std::to_string(count) + "_" + file_name_temp + "_inter" + start_time;
    } else {
        file_out_prefix = std::to_string(count) + "_" + file_name_temp;
    }

    return file_out_prefix;
}